#ifndef __POWER__H__
#define __POWER__H__

// Total Skills Time is 60 seconds
// Record motor power every 250 msecs = 60 * 4 = 240
#define MAX_ARRAY_SIZE 80

// defines a structure that willbe used by each motor to record all possible values in sequence
typedef struct
{
	// the array that tracks all of the motor values
	int MotorValue[MAX_ARRAY_SIZE];
	// the value that tracks which position in the array to add the value to
	int Position;
} Power;

typedef struct
{
	// encoder value array
	int EncoderValue[MAX_ARRAY_SIZE];
	// the value that tracks which position in the array to add the value to
	int Position;
} Encoder;

// reset the array of values for this motor
void ResetPower(Power &power)
{
	for(int i = 0; i < MAX_ARRAY_SIZE; i++)
	{
		power.MotorValue[i] = 0;
	}
}

// reset the array of values for this encoder
void ResetEncoder(Encoder &theEncoder)
{
	for(int i = 0; i < MAX_ARRAY_SIZE; i++)
	{
		theEncoder.EncoderValue[i] = 0;
	}
}

void Record(Power &power, int motorValue)
{
	if(power.Position + 1 >= MAX_ARRAY_SIZE)
	{
		// the top boundary of the array has been reached - do not store this value
		return;
	}
	// store the motor value into the array at the correct position
	power.MotorValue[power.Position] = motorValue;
	// advance the position of this array to the next value
	power.Position++;
}

void RecordEncoder(Encoder &theEncoder, int encoderValue)
{
	if(theEncoder.Position + 1 >= MAX_ARRAY_SIZE)
	{
		// the top boundary of the array has been reached - do not store this value
		return;
	}
	// store the motor value into the array at the correct position
	theEncoder.EncoderValue[theEncoder.Position] = encoderValue;
	// advance the position of this array to the next value
	theEncoder.Position++;
}

void Dump(const Power &power, int motorNumber)
{
	writeDebugStreamLine("MotorPower %d: { ", motorNumber);
	// begin writing out each value in the array
	for ( int index = 0; index < power.Position ; index++)
	{
		writeDebugStream("%d", power.MotorValue[index]);
		// if the next position is still less than the top bounds of the array, add a comma (before the next value gets added)
		if( (index + 1) < (power.Position) )
		{
			writeDebugStream(", ");
		}
		// add line break if this the next ten numbers
		if ( ((index % 10) == 0) && (index != 0) )
		{
			writeDebugStream("\n");
		}
	}
	writeDebugStreamLine("\n }");
}

void DumpEncoder(const Encoder &theEncoder, int encoderNumber)
{
	writeDebugStreamLine("Encoder Values %d: { ", encoderNumber);
	// begin writing out each value in the array
	for ( int index = 0; index < theEncoder.Position ; index++)
	{
		writeDebugStream("%d", theEncoder.EncoderValue[index]);
		// if the next position is still less than the top bounds of the array, add a comma (before the next value gets added)
		if( (index + 1) < (theEncoder.Position) )
		{
			writeDebugStream(", ");
		}
		// add line break if this the next ten numbers
		if ( ((index % 10) == 0) && (index != 0) )
		{
			writeDebugStream("\n");
		}
	}
	writeDebugStreamLine("\n }");
}

// load the value provided at the position provided
void LoadValue(Power &power, int position, int motorValue)
{
	if(position + 1 >= MAX_ARRAY_SIZE)
	{
		// the top boundary of the array has been reached - do not store this value
		return;
	}
	power.MotorValue[position] = motorValue;
}

// load the value provided at the position provided
void LoadEncoderValue(Encoder &theEncoder, int position, int encoderValue)
{
	if(position + 1 >= MAX_ARRAY_SIZE)
	{
		// the top boundary of the array has been reached - do not store this value
		return;
	}
	theEncoder.EncoderValue[position] = encoderValue;
}

#endif
